#include "StdAfx.h"
#include "colisionador.h"
#include "semirrecta2d.h"
#include "vector2d.h"
#include "matematicas.h"

Colisionador::Colisionador()
{

}

Colisionador::~Colisionador()
{

}

Colision Colisionador::calcularColision(Bola* bola, Ente* entePasivo)
{
    Arista* arista = dynamic_cast<Arista*>(entePasivo);
    if (arista != NULL) {
        Colision colision = calcularColisionBolaArista(bola, arista);

        if (!colision.esValida()) {
            colision = calcularColisionBolaVertice(bola, arista->getSegmento().getP1());
        }
        if (!colision.esValida()) {
            colision = calcularColisionBolaVertice(bola, arista->getSegmento().getP2());
        }
        return colision;
    } 
    
    Bola* otraBola = dynamic_cast<Bola*>(entePasivo);
    if (otraBola != NULL) {
        return calcularColisionBolas(bola, otraBola);
    }
    return Colision();
}

/**
 * Calcula la variable lambda del sistema de ecuaciones:
 * P1 = lambda * VD1 + O1
 * P2 = lambda * VD2 + O2
 * |P1 P2| = r1 + r2
 * Dondde:
 * P1 = posicion de choque bola 1
 * P2 = posicion de choche bola 2
 * r1 = radio bola 1
 * r2 = radio bola 2
 * VD1 = vector desplazamiento bola 1
 * VD2 = vector desplazamiento bola 2
 * O1 = posicion inicial bola 1
 * O2 = posicion final bola 2
 */
double Colisionador::calcularLambdaColisionBolas(Bola* bolaActiva, Bola* otraBola)
{
    Vector2d vectorDesplazamiento1 = bolaActiva->getVectorDesplazamiento();

    double dx1 = vectorDesplazamiento1.getX();
    double dy1 = vectorDesplazamiento1.getY();
    double ox1 = bolaActiva->getPosicion().getX();
    double oy1 = bolaActiva->getPosicion().getY();

    Vector2d vectorDesplazamiento2 = otraBola->getVectorDesplazamiento();
    double dx2 = 0;//vectorDesplazamiento2.getX();
    double dy2 = 0;//vectorDesplazamiento2.getY();
    double ox2 = otraBola->getPosicion().getX();
    double oy2 = otraBola->getPosicion().getY();

    double rt = bolaActiva->getRadio() + otraBola->getRadio();

    double dx = dx1 - dx2;
    double dy = dy1 - dy2;
    double ox = ox1 - ox2;
    double oy = oy1 - oy2;

    double a = dx * dx + dy * dy;
    double b = 2 * (dx * ox + dy * oy);
    double c = ox * ox + oy * oy - rt * rt;

    double lambda;

    if (a == 0) {
        if (b == 0) {
            return DOUBLE_NAN;
        }
        lambda = -c / b;
    } else {
        double b4ac = b * b - 4 * a * c;
        if (b4ac < 0) {
            return DOUBLE_NAN;
        }
        double raiz = sqrt(b4ac);
        double lambda1 = (-b + raiz) / 2 / a;
        double lambda2 = (-b - raiz) / 2 / a;
        if (lambda1 < 0) {
            lambda = lambda2;
        } else if (lambda2 < 0) {
            lambda = lambda1;
        } else {
            lambda = MIN(lambda1, lambda2);
        }
    }

    return lambda;
}

Colision Colisionador::calcularColisionBolas(Bola* bolaActiva, Bola* otraBola)
{

    if (Vector2d::sonIguales(bolaActiva->getVelocidad(), otraBola->getVelocidad())) {
        return Colision();
    }
    double lambda = calcularLambdaColisionBolas(bolaActiva, otraBola);
    if (esNan(lambda) || lambda < 0 || lambda > 1) {
        return Colision();
    }

    Vector2d vectorDesplazamiento1 = bolaActiva->getVectorDesplazamiento();

    double dx1 = vectorDesplazamiento1.getX();
    double dy1 = vectorDesplazamiento1.getY();
    double ox1 = bolaActiva->getPosicion().getX();
    double oy1 = bolaActiva->getPosicion().getY();

    Vector2d vectorDesplazamiento2 = otraBola->getVectorDesplazamiento();
    double dx2 = 0;// vectorDesplazamiento2.getX();
    double dy2 = 0;//vectorDesplazamiento2.getY();
    double ox2 = otraBola->getPosicion().getX();
    double oy2 = otraBola->getPosicion().getY();

    double x1 = lambda * dx1 + ox1;
    double y1 = lambda * dy1 + oy1;

    double x2 = lambda * dx2 + ox2;
    double y2 = lambda * dy2 + oy2;

    // Las posiciones de colision son las que ocupan los entes en el instante
    // en que se produce la colision
    Punto2d posicionColision1(x1, y1);
    Punto2d posicionColision2(x2, y2);

    double distanciaColision = Punto2d::distancia(bolaActiva->getPosicion(), posicionColision1);

    Recta2d rectaUnion(posicionColision1, posicionColision2);
    Punto2d puntoColision = rectaUnion.calcularPuntoDistancia(bolaActiva->getRadio());

    Colision colision;
    colision.setPuntoColision(puntoColision);
    colision.setEnteActivo(bolaActiva);
    //colision.setDistancia(distanciaColision);

    double distanciaPrevista = vectorDesplazamiento1.getModulo();
    colision.setProporcionDesplazamientoRealizado(distanciaPrevista == 0 ? 1 : distanciaColision / distanciaPrevista);

    colision.setPosicionColisionEnteActivo(posicionColision1);

    Vector2d direccionChoque(bolaActiva->getPosicion(), otraBola->getPosicion());

    Semirrecta2d semirectaFuerzaEntePasivo(otraBola->getPosicion(), direccionChoque);

    Vector2d fuerzaAplicadaEntePasivo = semirectaFuerzaEntePasivo.proyectar(bolaActiva->getFuerzaCinetica());
    if (!fuerzaAplicadaEntePasivo.esValido()) {
        return Colision();
    }
    fuerzaAplicadaEntePasivo.multiplicar(bolaActiva->getMaterial().getElasticidad() * otraBola->getMaterial().getElasticidad());

    colision.setEntePasivo(otraBola);
    colision.setAceleracionEntePasivo(fuerzaAplicadaEntePasivo.multiplicar(1 / otraBola->getMasa()));
    //colision.setPosicionColisionEntePasivo(posicionColision2);

    //if (distanciaColision > 0) {
    Vector2d fuerzaAplicadaEnteActivo = fuerzaAplicadaEntePasivo.getVectorInvertido().sumar(bolaActiva->getFuerzaCinetica());
    fuerzaAplicadaEnteActivo.multiplicar(bolaActiva->getMaterial().getElasticidad() * otraBola->getMaterial().getElasticidad());
    colision.setAceleracionEnteActivo(fuerzaAplicadaEnteActivo.multiplicar(1 / bolaActiva->getMasa()));
    //}

    return colision;
}

Colision Colisionador::calcularColisionBolaArista(Bola* bola, Arista* arista)
{
    Vector2d vectorDesplazamiento = bola->getVectorDesplazamiento();

    Recta2d lineaDireccion(bola->getPosicion(), vectorDesplazamiento);
    Recta2d lineaSegmento(arista->getSegmento().getP1(), arista->getSegmento().getP2());
    Punto2d puntoCorte = lineaSegmento.interseccion(lineaDireccion);
    if (!puntoCorte.esValido()) {
        return Colision();
    }

    Angulo anguloIncidencia = lineaSegmento.calcularMenorAngulo(lineaDireccion);

    double hipotenusa = bola->getRadio() / sin(anguloIncidencia.getRadianes());

    Vector2d vectorHipotenusa = vectorDesplazamiento.getVectorInvertido();
    vectorHipotenusa.setModulo(hipotenusa);
    Punto2d posicionColision = vectorHipotenusa.trasladarPunto(puntoCorte);

    Vector2d vectorDireccionColision(bola->getPosicion(), posicionColision);
    double distanciaColision = vectorDireccionColision.getModulo();

    if (!vectorDireccionColision.esNulo()
        && !Angulo::sonIguales(vectorDireccionColision.getAngulo(), vectorDesplazamiento.getAngulo())) {
        return Colision();
    }

    double distanciaPrevista = vectorDesplazamiento.getModulo();
    double proporcionMovimientoRealizado = distanciaColision / distanciaPrevista;

    if (!fequals(proporcionMovimientoRealizado, 1) && proporcionMovimientoRealizado > 1) {
        return Colision();
    }

    // Esto evita que las bolas contiguas generen una colision
//        if (Geometria.iguales(distanciaColision, 0)) {
//            return null;
//        }

    Recta2d perpendicular = lineaSegmento.perpendicular(posicionColision);
    Punto2d puntoColision = lineaSegmento.interseccion(perpendicular);
    if (!puntoColision.esValido() || !arista->getSegmento().contiene(puntoColision)) {
        return Colision();
    }

    Colision colision;
    colision.setPuntoColision(puntoColision);
    colision.setProporcionDesplazamientoRealizado(proporcionMovimientoRealizado);

    colision.setEnteActivo(bola);
    colision.setPosicionColisionEnteActivo(posicionColision);
    //colision.setDistancia(distanciaColision);

    Angulo anguloSegmento = lineaSegmento.getAngulo();
    Angulo anguloEntrada = Angulo::resta(vectorDesplazamiento.getAngulo(), anguloSegmento);
    Angulo anguloSalida(-anguloEntrada.getGrados());
    anguloSalida.sumar(anguloSegmento);
    Vector2d aceleracionResultante(anguloSalida, vectorDesplazamiento.getModulo());
    aceleracionResultante = aceleracionResultante.multiplicar(bola->getMaterial().getElasticidad() * arista->getMaterial().getElasticidad());
//        if (aceleracionResultante.getModulo() < 1)
//            return null;

    colision.setAceleracionEnteActivo(aceleracionResultante);

    return colision;
}

Colision Colisionador::calcularColisionBolaVertice(Bola* bola, const Punto2d& puntoVertice)
{
    double elasticidadVertice = 1; // TODO: pasa como parametro la elasticidad del vertice
    Vector2d vectorDesplazamiento = bola->getVectorDesplazamiento();

    if (vectorDesplazamiento.esNulo()) {
        return Colision();
    }
    Recta2d lineaDireccion(bola->getPosicion(), vectorDesplazamiento);
    Recta2d lineaPerpendicular = lineaDireccion.perpendicular(puntoVertice);

    Punto2d interseccion = lineaDireccion.interseccion(lineaPerpendicular);
    if (!interseccion.esValido()) {
        //System.out.println("error: dos rectas perpendiculares deben tener interseccion");
        return Colision();
    }

    Vector2d direccionColision(bola->getPosicion(), interseccion);
    if (!Angulo::sonIguales(direccionColision.getAngulo(), vectorDesplazamiento.getAngulo())) {
        return Colision();
    }

    double separacionLateral = Punto2d::distancia(puntoVertice, interseccion);

    if (separacionLateral > bola->getRadio()) {
        return Colision();
    }

    double separacionFrontal = sqrt(bola->getRadio() * bola->getRadio() - separacionLateral * separacionLateral);

    double distanciaColision = Punto2d::distancia(bola->getPosicion(), interseccion) - separacionFrontal;

    if (distanciaColision > vectorDesplazamiento.getModulo()/* || distanciaColision <= 0*/) {
        return Colision();
    }

    Vector2d vectorDesplazamientoColision =
            vectorDesplazamiento.multiplicar(bola->getMaterial().getElasticidad() * elasticidadVertice);
    vectorDesplazamientoColision.setModulo(distanciaColision);
    Punto2d posicionColision = vectorDesplazamientoColision.trasladarPunto(bola->getPosicion());

    double distanciaPrevista = vectorDesplazamiento.getModulo();
    double proporcionMovimientoRealizado = distanciaColision / distanciaPrevista;

    if (!fequals(proporcionMovimientoRealizado, 1) && proporcionMovimientoRealizado > 1) {
        return Colision();
    }

    Colision colision;
    colision.setPuntoColision(posicionColision);
    colision.setProporcionDesplazamientoRealizado(proporcionMovimientoRealizado);

    colision.setEnteActivo(bola);
    colision.setPosicionColisionEnteActivo(posicionColision);
    //colision.setDistancia(distanciaColision);
//        colision.setPuntoColision(puntoVertice);

    Vector2d vectorRadio(posicionColision, puntoVertice);
    Angulo anguloRadio = Angulo::resta(vectorDesplazamiento.getAngulo(), vectorRadio.getAngulo());
    double radianes = vectorDesplazamiento.getAngulo().getRadianes();
    double radianesDesviacion = PI - 2 * asin(separacionLateral / bola->getRadio());
    if (anguloRadio.getGrados() >= 180) {
        radianes -= radianesDesviacion;
    } else {
        radianes += radianesDesviacion;
    }

    Angulo anguloRebote;
    anguloRebote.setRadianes(radianes);
    Vector2d aceleracionResultante(anguloRebote, vectorDesplazamiento.getModulo());
    colision.setAceleracionEnteActivo(aceleracionResultante);

    return colision;
}
